﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Windows;
using Microsoft.Kinect;
using Microsoft.Samples.Kinect.KinectExplorer;
using Microsoft.Samples.Kinect.WpfViewers;

namespace ZeroTouchUI
{
    /// <summary>
    /// Represents the bounding box of the mapped points
    /// </summary>
    public struct BoundingBox
    {
        public double width;
        public double height;
        public Point minPoint; // Upper left corner of the box
        public Point maxPoint; // Lower right corner or the box
    }

    /// <summary>
    /// Functions for the implementation of the $1 algrotihm,
    /// as presented by Jacob O. Wobbrock, Andrew D. Wilson,
    /// and Yang Li, UIST 2007
    /// </summary>
    public static class Dollar
    {
        public static Dictionary<Gesture, Action> templates = new Dictionary<Gesture,Action>();
        private static readonly int size = 250;
        private static Mutex mutex = new Mutex();

        //public static List<Point> Interpolate(List<Point> points, int sampleSize)
        //{
            
        //}

        public static List<Point> Resample(List<Point> points)
        {
            return Resample(points, 10);
        }

        public static List<Point> Resample(List<Point> points0, int sampleSize)
        {
            List<Point> returned = new List<Point>();
            mutex.WaitOne();
            if (points0 == null || points0.Count <= 0 || sampleSize <= 0)
            {
                return points0;
            }

            List<Point> points = new List<Point>();
            points.AddRange(points0);

            double interval = (GetPathLength(points) / (double)(sampleSize - 1));
            double localDist = 0;
            List<Point> newPoints = new List<Point> { points[0] };

            for (int i = 1; i < points.Count; i++)
            { 
                Point point = points[i], prev = points[i - 1];
                double curDist = GetDistance(point, prev);
                if (localDist + curDist >= interval)
                {
                    Point newpoint = new Point();
                    if (curDist != 0)
                    {
                        newpoint = new System.Windows.Point(
                            prev.X + (((interval - localDist) / curDist) * (point.X - prev.X)),
                            prev.Y + (((interval - localDist) / curDist) * (point.Y - prev.Y)));
                    }
                    else
                    {
                        newpoint = new System.Windows.Point(point.X, point.Y);
                    }

                    newPoints.Add(newpoint);
                    points.Insert(i, newpoint);
                    localDist = 0;
                }
                else
                {
                    localDist += curDist;
                }
            }

            if (newPoints == null || newPoints.Count == 0)
            {
                newPoints = new List<Point>();
                for (int i = 0; i < sampleSize; i++)
                {
                    newPoints.Add(points[i]);
                }
            }

            while (newPoints.Count < sampleSize)
            { 
                int i = Math.Min(points.Count - (sampleSize -  newPoints.Count), points.Count - 1);
                newPoints.Add(points[i]);
            }

            returned.AddRange(newPoints);
            mutex.ReleaseMutex();

            return returned;
        }

        private static double GetPathLength(List<Point> points)
        {
            double dist = 0;
            Point prev = points[0];

            foreach (Point point in points.Skip(1)) //skips first element since its distance is 0
            {
                dist += GetDistance(point, prev);
                prev = point;
            }

            return dist;
        }

        private static double GetDistance(Point pointA, Point pointB)
        {
            return Math.Sqrt(Square(pointA.X - pointB.X) + Square(pointA.Y - pointB.Y));
        }

        private static double Square(double x)
        {
            return x * x;
        }

        public static List<Point> TranslateToOrigin(List<Point> points)
        {
            BoundingBox bb = GetBoundingBox(points);
            List<Point> newPoints = new List<Point>();
            foreach (Point point in points)
            {
                newPoints.Add(new Point
                {
                    X = point.X - bb.minPoint.X + 5.0,
                    Y = point.Y - bb.minPoint.Y + 5.0
                });
            }

            return newPoints;
        }

        public static List<Point> ScaleWithSharpen(List<Point> points)
        {
            BoundingBox oldBb = GetBoundingBox(points);

            if (oldBb.width > (oldBb.height * 5.0))
            {
                double average = 0;
                foreach (Point p in points)
                {
                    average += p.Y;
                }
                average /= (double)(points.Count);
                List<Point> sharpen = new List<Point>();
                foreach (Point p in points)
                {
                    sharpen.Add(new Point { X = p.X, Y = average });
                }
                points = sharpen;
            }
            else if (oldBb.height > (oldBb.width * 5.0))
            {
                double average = 0;
                foreach (Point p in points)
                {
                    average += p.X;
                }
                average /= (double)(points.Count);
                List<Point> sharpen = new List<Point>();
                foreach (Point p in points)
                {
                    sharpen.Add(new Point { X = average, Y = p.Y });
                }
                points = sharpen;
            }

            return Scale(points, oldBb);
        }

        public static List<Point> Scale(List<Point> points)
        {
            BoundingBox oldBb = GetBoundingBox(points);
            return Scale(points, oldBb);
        }

        public static List<Point> Scale(List<Point> points, BoundingBox oldBb)
        {
            List<Point> newPoints = new List<Point>();
            if (oldBb.width > (oldBb.height * 5.0))
            {
                foreach (Point point in points)
                {
                    newPoints.Add(new Point
                    {
                        X = point.X * (size / oldBb.width),
                        Y = 5
                    });
                }
            }
            else if (oldBb.height > (oldBb.width * 5.0))
            {
                foreach (Point point in points)
                {
                    newPoints.Add(new Point
                    {
                        X = 5,
                        Y = point.Y * (size / oldBb.height)
                    });
                }
            }
            else
            {
                foreach (Point point in points)
                {
                    newPoints.Add(new Point
                    {
                        X = point.X * (size / oldBb.width),
                        Y = point.Y * (size / oldBb.height)
                    });
                }
            }

            return newPoints;
        }

        public static BoundingBox GetBoundingBox(List<Point> points)
        {
            List<double> xVals = new List<double>(), yVals = new List<double>();
            foreach (Point point in points)
            {
                xVals.Add(point.X);
                yVals.Add(point.Y);
            }

            double xMax = xVals.Max();
            double yMax = yVals.Max();
            double xMin = xVals.Min();
            double yMin = yVals.Min();

            return new BoundingBox
            {
                height = yMax - yMin,
                width = xMax - xMin,
                minPoint = new Point
                {
                    X = xMin,
                    Y = yMin
                },
                maxPoint = new Point
                {
                    X = xMax,
                    Y = yMax
                }
            };
        }

        public static BoundingBox MergeBounds(List<Point> left, List<Point> right)
        {
            if (left == null || left.Count == 0)
            {
                return GetBoundingBox(right);
            }
            if (right == null || right.Count == 0)
            {
                return GetBoundingBox(left);
            }
            BoundingBox bba = GetBoundingBox(left);
            BoundingBox bbb = GetBoundingBox(right);
            BoundingBox bbc = new BoundingBox
            {
                minPoint = new Point
                {
                    X = Math.Min(bba.minPoint.X, bbb.minPoint.X),
                    Y = Math.Min(bba.minPoint.Y, bba.minPoint.Y)
                },
                maxPoint = new Point
                {
                    X = Math.Max(bba.maxPoint.X, bbb.maxPoint.X),
                    Y = Math.Max(bba.maxPoint.Y, bba.maxPoint.Y)
                }
            };

            bbc.height = bbc.maxPoint.Y - bbc.minPoint.Y;
            bbc.width = bbc.maxPoint.X - bbc.minPoint.X;

            return bbc;
        }

        public static Gesture Recognize(List<Point> left, List<Point> right, double strength)
        {
            if (templates == null || templates.Keys.Count == 0)
            {
                return null;
            }

            BoundingBox leftBB = new BoundingBox { width = 0, height = 0 }, rightBB = new BoundingBox { width = 0, height = 0 };
            if (left == null || left.Count <= 0)
            {
                left = null;
            }
            else
            {
                leftBB = GetBoundingBox(left);
            }
            if (right == null || right.Count <= 0)
            {
                right = null;
            }
            else
            {
                rightBB = GetBoundingBox(right);
            }

            if (left == null || (leftBB.height < 100 && leftBB.width < 100))
            {
                left = new List<Point>();
            }

            if (right == null || (rightBB.height < 100 && rightBB.width < 100))
            {
                right = new List<Point>();
            }

            Gesture bestMatch = null;
            double bestScore = 0;

            foreach (Gesture template in templates.Keys)
            {
                double curDist = Double.PositiveInfinity;
                for (int i = 20; i > 5; i -= 5)
                {
                    List<Point> leftI = new List<Point>(), rightI = new List<Point>(),
                                leftT = new List<Point>(), rightT = new List<Point>();

                    if (left.Count > 0)
                    {
                        leftI = Resample(left, i);
                        leftI = ScaleWithSharpen(leftI);
                        leftI = TranslateToOrigin(leftI);
                    }
                    if (right.Count > 0)
                    {
                        rightI = Resample(right, i);
                        rightI = ScaleWithSharpen(rightI);
                        rightI = TranslateToOrigin(rightI);
                    }
                    if (template.leftGesture.Count > 0)
                    {
                        leftT = Resample(template.leftGesture, i);
                        leftT = ScaleWithSharpen(leftT);
                        leftT = TranslateToOrigin(leftT);
                    }
                    if (template.rightGesture.Count > 0)
                    {
                        rightT = Resample(template.rightGesture, i);
                        rightT = ScaleWithSharpen(rightT);
                        rightT = TranslateToOrigin(rightT);
                    }

                    curDist = Math.Min(PathDistance(leftI, leftT, i) + PathDistance(rightI, rightT, i), curDist);
                }
                
                if (template.leftGesture.Count > 0 && template.rightGesture.Count > 0)
                {
                    curDist /= 2.0;   // Take average of two-handed templates to avoid favoritism towards one-handed gestures
                }

                double score = 1 - (curDist / (0.5 * Math.Sqrt(Square(size) + Square(size))));
                if (score > bestScore)
                {
                    bestScore = score;
                    bestMatch = template;
                }
            }

            if (bestMatch != null && bestScore > strength)
            {
                //double curDist1, curDist2, curDist3, curDist4;
                //curDist1 = PathDistance(left, bestMatch.leftGesture, 20) + PathDistance(right, bestMatch.rightGesture, 20);
                //curDist2 = PathDistance(Resample(left, 15), Resample(bestMatch.leftGesture, 15), 15) + PathDistance(Resample(right, 15), Resample(bestMatch.rightGesture, 15), 15);
                //curDist3 = PathDistance(Resample(left, 10), Resample(bestMatch.leftGesture, 10), 10) + PathDistance(Resample(right, 10), Resample(bestMatch.rightGesture, 10), 10);
                //curDist4 = PathDistance(Resample(left, 5), Resample(bestMatch.leftGesture, 5), 5) + PathDistance(Resample(right, 5), Resample(bestMatch.rightGesture, 5), 5);
                //Console.WriteLine("Recognized " + bestMatch.Name + " with score " + bestScore.ToString());
                return bestMatch;
            }
            else
            {
                return null;
            }
        }

        private static double PathDistance(List<Point> inp, List<Point> templ, int sampleSize)
        {
            double dist = 0;
            
            if (templ.Count == 0)
            {
                return 0;
            }

            List<Point> input = Resample(inp, sampleSize);
            List<Point> template = Resample(templ, sampleSize);
            double gestureSize = (double)Math.Min(input.Count, template.Count);

            for (int i = 0; i < gestureSize; i++)
            {
                dist += GetDistance(input.ElementAt(i), template.ElementAt(i));
            }

            return (dist / gestureSize);
        }
    }
}
